#ifndef _VEHICLE_H_
#define _VEHICLE_H_

#include <utilities/utilities.h>
#include <vector>
#include <map>
#include <string>
#include <dispatch_system/definitions.h>
#include <dispatch_system/map_object.h>
#include <moying_proto/navigation.pb.h>

namespace moying
{
namespace navigation
{
    //导航模式信息
    struct NavModeInfo{
        moying_proto::NavMode nav_mode;     //导航模式
        unsigned int nav_wait_timeout;      //等待障碍物移除的超时时间,单位秒
        NavModeInfo(){};
        NavModeInfo(moying_proto::NavMode mode,unsigned int timeout) : nav_mode(mode),nav_wait_timeout(timeout){}
    };

    class Vehicle: public MapObject
    {
        public:
            static MovePattern getMovePattern(MovePatternType move_pattern_type);

            Vehicle();
            Vehicle(std::string type, const std::string &name);
            Vehicle(std::string type, const std::string &name, unsigned int id);
            virtual ~Vehicle();

            virtual bool isOmniVehicle();
            virtual void setMovePatterns();
            virtual MovePatternsPtr getValidMovePatterns(MovePatternState sm_state);
            virtual bool isMovePatternValid(MovePatternType type,MovePatternState sm_state);
            virtual double getTheta(int dir);
            virtual int getCloseTheta(double th);
            virtual double getVelocity(int sm);
            virtual void getNearNodes(const VehicleState &state, std::vector<Node> &nodes);
            virtual void constructPlanFromGraphPath(MoyingMapPtr moyingmap, const VehicleState &start, const VehicleState &goal, const std::vector<Node> &node_path, PlanningResult &result);
            virtual unsigned int getStartNodeIndex(const VehicleState &start_state, char &sm);
            virtual unsigned int getGoalNodeIndex(const VehicleState &start_state, char &sm);
            inline unsigned int getID()
            {
                return id_;
            }

            inline double getLength()
            {
                return length_;
            }

            inline double getWidth()
            {
                return width_;
            }

            inline double getHeight()
            {
                return height_;
            }
             
            inline std::string getRobotType()
            {
                return robot_type_;
            }

            inline void setRobotSize(double l,double w,double h){
                length_= l;
                width_ = w;
                height_= h;
            }

            inline bool recv_path_plan_req(){return recv_path_plan_req_;}
            inline void set_recv_path_plan_req(bool val){recv_path_plan_req_=val;}
            inline int last_path_plan_result(){return last_path_plan_result_;}
            inline void set_last_path_plan_result(int val){last_path_plan_result_=val;}
            inline int path_plan_fail_times(){return path_plan_fail_times_;}
            inline void set_path_plan_fail_times(int value){path_plan_fail_times_=value;}
            inline void set_nav_fail_times(int times){nav_fail_times_=times;}
            inline int nav_fail_times(){return nav_fail_times_;}

            void set_obstacle_info(const LocalObstacleInfo& obs_info);
            const LocalObstacleInfo& obstacle_info();
            // void cacheObstacleInfo(const LocalObstacleInfo& obs_info);
            inline void set_path_plan_result(navigation::PlanningResult result){
                path_plan_result_.clear();
                path_plan_result_ = result;
            }
            navigation::PlanningResult& path_plan_result(){return path_plan_result_;}

            bool getFootprintOccupiedCells(const CostMapPtr &costmap, std::map<int, FootprintCell> &footprint_cells);
            bool getFootprintOccupiedCells(const CostMapPtr &costmap, int x, int y, std::map<int, FootprintCell> &footprint_cells,float theta=0);
            bool getFootprintOccupiedCells(const CostMapPtr &costmap, const NodePtr &node, std::map<int, FootprintCell> &footprint_cells);
            bool getFootprintOccupiedCells(const CostMapPtr &costmap, const VehicleState &state, std::map<int, FootprintCell> &footprint_cells);

            bool getUnpaddedFootprintOccupiedCells(const CostMapPtr &costmap, int x, int y, std::map<int, FootprintCell> &footprint_cells);
            bool getPathFootprintOccupiedCells(const CostMapPtr &costmap, const NodePtr &a, const NodePtr &b, std::map<int, FootprintCell> &footprint_cells);
            void updatePose(double x, double y, double th);
            void getPose(VehicleState &state);
            VehicleState getPose();
            
            void set_nav_mode_info(NavModeInfo info);
            NavModeInfo nav_mode_info(){return nav_mode_info_;}
            
            /**
             * @brief isRobotCanNav  检测机器人是否可以导航 
             * @return  true:可以导航，false：不可以导航
             */ 
            bool isRobotCanNav();

            /**
             * @brief 机器人是否遇到了紧急障碍物 
             * @return  true:有，false：没有
             */ 
            bool isRobotInEmergencyObstacle();

            //更新机器人当前的足迹占用
            void updateCurFootprint(const CostMapPtr &costmap);

            std::map<int, FootprintCell>& cur_footprint_cell(){return cur_footprint_cell_;}
            VehicleState global_goal(){return global_goal_;}
            void set_global_goal(VehicleState global_goal){global_goal_=global_goal;}

            int one_way_road_id(){return one_way_road_id_;}
            void set_one_way_road_id(int id){one_way_road_id_=id;}

            TimedFootprints& temp_time_occupy(){return temp_time_occupy_;}
            void set_occupy_time_after_arrive(unsigned int val){occupy_time_after_arrive_ = val;}
            unsigned int occupy_time_after_arrive(){ return occupy_time_after_arrive_;}

            void set_deadlock_group_id(int id){deadlock_group_id_ = id;}
            int deadlock_group_id(){return deadlock_group_id_;}

        private:
            void init();

        protected:
            unsigned int id_;
            double length_;
            double width_;
            double height_;
            std::string robot_type_;
            std::map<int, MovePatternsPtr> move_patterns_;
            std::recursive_mutex obstacle_info_mutex_;
            LocalObstacleInfo obstacle_info_;   //局部障碍物
            // LocalObstacleInfo obstacle_info_cache_;   //局部障碍物

            NavModeInfo nav_mode_info_;     //导航模式信息
            VehicleState state_;
            VehicleState global_goal_;      //全局导航目标点
            std::map<int, FootprintCell>    cur_footprint_cell_;    //机器人当前的footprint
            static std::vector<MovePattern> move_pattern_vec_;  //基本的运动模式

            bool recv_path_plan_req_ = false;
            int last_path_plan_result_ = 1; //1:成功，其他值失败（2：目标点在障碍物里，3：规划超时，4：无法生成到目标点的路径,5：距离障碍物太近,6:单向道不可通行）
            int path_plan_fail_times_ = 0;   //连续规划路径失败的次数
            int nav_fail_times_ = 0;        //连续导航失败的次数

            int one_way_road_id_ = 0;   //所在单向道id,0表示没有在单向道

            TimedFootprints temp_time_occupy_;//临时的时间占用信息
            unsigned int occupy_time_after_arrive_=0;  //到达目标点后持续占用时间地图的秒数

            navigation::PlanningResult path_plan_result_;   //路径规划结果，当路径点数量大于0时，该字段才有效
            int deadlock_group_id_ = 0; //死锁机器人组id,0表示机器人没有在死锁组中
    };
    typedef std::shared_ptr<Vehicle> VehiclePtr;
}
}
#endif //_VEHICLE_H_